//
// Created by 74354 on 2022/1/1.
//


/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
#include "commuTask.h"
#include "myPrint.h"
#include "main.h"
#include "Encoder.hpp"
#include "canConfig.h"
#include "gdtr_define.h"
#include "stm32f405xx.h"
#include "boardInit.h"
#include "printf.h"


#define EN_USART true
#define EN_CAN  false
#define CAN_ID_MOTOR0_ID 0X112233AA
#define CAN_ID_MOTOR1_ID 0X112233BB

#define CAN_STDID_OFFSET 18


extern osThreadId_t commuTaskHandler;
extern osMessageQueueId_t msgMotorInfo_id; 
extern osMessageQueueId_t msgLogInfo_id;
extern osSemaphoreId smpMotorReady_id;

static char *logInfo;
msgMotorInfoStructure motorInfo;
float motor0_rcv_angle;
float motor1_rcv_angle;
CAN can;

#define MSG_LogSendWaitTicks 0
#define MSG_LogGetWaitTicks 0
#define MSG_MotorGetWaitTicks 0

uint8_t sendLogMsg(osMessageQueueId_t msg_id,char** str)
{
    if(msg_id == NULL){
        USER_LOG(USER_LOG_ERR,"msg_is is NULL!");
        return -1;
    }
    if(osMessageQueuePut(msg_id,str,NULL,MSG_LogSendWaitTicks) != osOK){
        USER_LOG(USER_LOG_ERR,"Send msg failed!");
        free(*str);
        return -1;
    }
    return 0;
}

void CanReceiveCallback(uint16_t ID,const uint8_t* data,uint8_t len)
{
    float temp;
    if(ID == CAN_ID_MOTOR0_ID){
        if(len != 8){
            USER_LOG(USER_LOG_DEBUG,"Data length:%d wrong",len);
            return;
        }
        temp = *((float*)&data);
        if(temp != *((float*)&data[4])){
            USER_LOG(USER_LOG_DEBUG,"The data[0]~data[3]:%.2f don't equals to data[4]~data[7]:%.2f!",temp,*((float*)&data[4]));
            return;
        }
        motor0_rcv_angle = temp;
        return;
    }else if(ID == CAN_ID_MOTOR1_ID){
        if(len != 8){
            USER_LOG(USER_LOG_DEBUG,"Data length:%d wrong",len);
            return;
        }
        temp = *((float*)&data);
        if(temp != *((float*)&data[4])){
            USER_LOG(USER_LOG_DEBUG,"The data[0]~data[3]:%.2f don't equals to data[4]~data[7]:%.2f!",temp,*((float*)&data[4]));
            return;
        }
        motor1_rcv_angle = temp;
        return;
    }
}

void commuTaskFunc(void *argument)
{

    osDelay(10);
    uint8_t ret;
    CAN_User_Init(&BOARD_CAN);
    can.handleCAN = BOARD_CAN;
    float test = 0.001;
    osSemaphoreAcquire(smpMotorReady_id,osWaitForever);
    for(;;){
        

        test += 0.1;
        // USER_LOG(USER_LOG_INFO,"mask:%f",test);
        // CANx_SendNormalData(&BOARD_CAN,1234,a,8);
        // can.sendDataFrame(Expanded,0x1fffffff,a,8);
        
        // if(osMessageQueueGet(msgLogInfo_id,(void **)&logInfo,NULL,MSG_LogGetWaitTicks) != osOK){
        //     USER_LOG(USER_LOG_EXCEPTION,"Get msgLogInfo_id failed");
        // }else{
        //     USER_LOG(USER_LOG_INFO,"%s",logInfo);
        //     free(logInfo);
        // }

        

        if(osMessageQueueGet(msgMotorInfo_id,&motorInfo,NULL,MSG_MotorGetWaitTicks) != osOK){
            USER_LOG(USER_LOG_EXCEPTION,"Get msgMotorInfo_id failed!");
        }else{
            switch(motorInfo.motor_id){
                case 0:
                    can.sendDataFrame(Expanded,CAN_ID_MOTOR0_ID,(uint8_t*)&motorInfo.motorAngle,8);
                    USER_LOG(USER_LOG_EXCEPTION,"mask:%.2f",motorInfo.I_current);
                    break;
                case 1:
                    can.sendDataFrame(Expanded,CAN_ID_MOTOR1_ID,(uint8_t*)&motorInfo.motorAngle,8);
                    break;
                default:break;
            }
        }
        osDelay(10);
    }
}





 



